#include <iostream>
#include <fstream>
#include <iomanip>    // For std::setw, std::fixed, std::setprecision, std::hex, std::dec
#include <vector>     // For std::vector
#include <algorithm>  // For std::min, std::max
#include <cmath>      // For fabs, roundf, powf, floorf
#include <limits>     // For std::numeric_limits
#include <string>     // For std::string
#include <cstdint>    // For int32_t, uint16_t, uint8_t

// 包含顶层HLS模块的头文件，HLS模块内部会使用ap_fixed/ap_uint
#include "gpu_render.hpp" // 包含 fpal.hpp, pbirsm.hpp, hr_tbr.hpp, tbr_rasterizer.hpp

// =========================================================================================
// PS端数据类型定义和转换函数 (模拟 HLS ap_fixed/ap_uint 的行为)
// -----------------------------------------------------------------------------------------
// PS端没有 ap_fixed.h，所以我们需要手动模拟其行为，特别是 bit-level 的转换。
// 这些函数将用于将 PS 端浮点/整数数据转换为 PL 端期望的底层位模式，反之亦然。
// =========================================================================================

// ==================== fp_coord_t (ap_fixed<31, 21, AP_RND_CONV, AP_SAT>) ====================
// HLS定义：typedef ap_fixed<31, 21, AP_RND_CONV, AP_SAT> fp_coord_t;
// W=31, I=21, F=10 (W-I)
const int PS_FP_COORD_W = 31; // Total bits
const int PS_FP_COORD_I = 21; // Integer bits (including sign)
const int PS_FP_COORD_F = PS_FP_COORD_W - PS_FP_COORD_I; // Fractional bits = 10
const float PS_FP_COORD_SCALE_FACTOR = powf(2.0f, PS_FP_COORD_F); // 2^10 = 1024.0f

// HLS ap_fixed<W,I> 内部存储为 W 位的有符号整数。
// 对于 31 位，其范围是 [-2^30, 2^30 - 1]。
const int64_t PS_FP_COORD_MAX_INTERNAL_VAL = (1LL << (PS_FP_COORD_W - 1)) - 1;   // max positive: 1073741823
const int64_t PS_FP_COORD_MIN_INTERNAL_VAL = -(1LL << (PS_FP_COORD_W - 1));     // min negative: -1073741824

/**
 * @brief 将 PS 端浮点数 (float) 转换为 HLS IP核期望的 31 位定点数内部整数表示 (int32_t)。
 *        模拟 ap_fixed 的舍入 (AP_RND_CONV, round half up) 和饱和 (AP_SAT) 行为。
 *        这个内部整数值会被写入 DDR/AXI-Lite。
 * @param val 输入浮点数
 * @param debug_prefix 调试信息前缀，用于区分不同参数
 * @param is_debug 是否开启调试打印
 * @return 转换为 31 位定点数的内部整数表示 (int32_t)。
 */
int32_t ps_float_to_fp_coord_internal(float val, const std::string& debug_prefix = "", bool is_debug = false) {
    if (is_debug) {
        std::cout << debug_prefix << "[DEBUG] PS Float input: " << std::fixed << std::setprecision(6) << val << std::endl;
    }

    // 1. Scale: 乘以 2^F
    float scaled_val = val * PS_FP_COORD_SCALE_FACTOR;
    if (is_debug) {
        std::cout << debug_prefix << "[DEBUG] Scaled float (val * 2^F): " << scaled_val << std::endl;
    }
    
    // 2. Rounding: AP_RND_CONV 是 round half to even (IEEE 754 standard rounding)
    // C++11 std::round 实现了 round half to even
    long long internal_val_before_sat = static_cast<long long>(std::round(scaled_val));
    if (is_debug) {
        std::cout << debug_prefix << "[DEBUG] Rounded to long long (before saturation): " << internal_val_before_sat << std::endl;
    }

    // 3. Saturation: 确保值在 31 位有符号整数的范围内
    long long internal_val = std::max(PS_FP_COORD_MIN_INTERNAL_VAL, std::min(internal_val_before_sat, PS_FP_COORD_MAX_INTERNAL_VAL));
    if (is_debug) {
        std::cout << debug_prefix << "[DEBUG] Saturated long long (final internal): " << internal_val << std::endl;
        std::cout << debug_prefix << "[DEBUG] PS_FP_COORD_MIN_INTERNAL_VAL: " << PS_FP_COORD_MIN_INTERNAL_VAL << ", PS_FP_COORD_MAX_INTERNAL_VAL: " << PS_FP_COORD_MAX_INTERNAL_VAL << std::endl;
        if (internal_val != internal_val_before_sat) {
            std::cout << debug_prefix << "[DEBUG] *** Saturation occurred! ***" << std::endl;
        }
    }
    
    return static_cast<int32_t>(internal_val); // 返回 31 位有符号整数
}

/**
 * @brief 将 HLS IP核的 31 位定点数内部整数表示 (int32_t) 转换回 PS 端浮点数 (float)。
 *        用于从 IP 核读取定点数值时进行解释。
 * @param internal_val IP核输出的 31 位定点数内部整数表示
 * @param debug_prefix 调试信息前缀
 * @param is_debug 是否开启调试打印
 * @return 转换后的浮点数
 */
float ps_fp_coord_internal_to_float(int32_t internal_val, const std::string& debug_prefix = "", bool is_debug = false) {
    if (is_debug) {
        std::cout << debug_prefix << "[DEBUG] PL int32_t internal value: " << internal_val << std::endl;
    }
    float converted_float = static_cast<float>(internal_val) / PS_FP_COORD_SCALE_FACTOR;
    if (is_debug) {
        std::cout << debug_prefix << "[DEBUG] Converted back to float: " << std::fixed << std::setprecision(6) << converted_float << std::endl;
    }
    return converted_float;
}


// ==================== fp_z_buffer_t (ap_uint<16>) ====================
// HLS定义：typedef ap_uint<16> fp_z_buffer_t;
// PS端直接使用 uint16_t 即可表示 16 位无符号整数
typedef uint16_t ps_fp_z_buffer_t;

/**
 * @brief 将 PS 端浮点数 (例如归一化深度) 转换为 16 位无符号 Z Buffer 值 (0-65535)。
 *        模拟 HLS ap_uint<16> 的截断行为。
 * @param z_float 输入浮点数
 * @param debug_prefix 调试信息前缀
 * @param is_debug 是否开启调试打印
 * @return 转换后的 16 位无符号 Z Buffer 值 (uint16_t)
 */
uint16_t ps_float_to_fp_z_buffer(float z_float, const std::string& debug_prefix = "", bool is_debug = false) {
    if (is_debug) {
        std::cout << debug_prefix << "[DEBUG] PS Float Z input: " << std::fixed << std::setprecision(6) << z_float << std::endl;
    }
    // HLS ap_uint<16>(float_val) 的行为是先取整（默认trunc），然后截断到16位。
    // 为了更接近 HLS 的 ap_uint(float) 行为，我们直接对浮点数进行截断（floor），然后进行钳制。
    float clamped_val = std::max(0.0f, std::min(z_float, 65535.0f)); // 钳制到 [0, 65535] 范围
    uint16_t result = static_cast<uint16_t>(floorf(clamped_val)); // 截断取整
    if (is_debug) {
        std::cout << debug_prefix << "[DEBUG] Clamped float Z (0-65535): " << std::fixed << std::setprecision(6) << clamped_val << std::endl;
        std::cout << debug_prefix << "[DEBUG] Final uint16_t Z value: " << result << std::endl;
    }
    return result;
}

/**
 * @brief 将 16 位无符号 Z Buffer 值转换回浮点数。
 * @param z_uint16 16 位无符号 Z Buffer 值
 * @return 转换后的浮点数
 */
float ps_fp_z_buffer_to_float(uint16_t z_uint16) {
    return static_cast<float>(z_uint16);
}


// ==================== fp_color_channel_t (ap_uint<8>) ====================
// HLS定义：typedef ap_uint<8> fp_color_channel_t;
// PS端直接使用 uint8_t 即可表示 8 位无符号整数
typedef uint8_t ps_fp_color_channel_t;

/**
 * @brief 将 PS 端浮点数或整数转换为 8 位无符号颜色通道值 (0-255)。
 *        模拟 HLS ap_uint<8> 的截断行为。
 * @param color_val 输入数值
 * @param debug_prefix 调试信息前缀
 * @param is_debug 是否开启调试打印
 * @return 转换后的 8 位无符号颜色通道值 (uint8_t)
 */
uint8_t ps_to_fp_color_channel(float color_val, const std::string& debug_prefix = "", bool is_debug = false) {
    if (is_debug) {
        std::cout << debug_prefix << "[DEBUG] PS Float Color input: " << std::fixed << std::setprecision(6) << color_val << std::endl;
    }
    // 钳制到 [0, 255] 范围
    float clamped_val = std::max(0.0f, std::min(color_val, 255.0f));
    uint8_t result = static_cast<uint8_t>(floorf(clamped_val)); // 截断取整
    if (is_debug) {
        std::cout << debug_prefix << "[DEBUG] Clamped float Color (0-255): " << std::fixed << std::setprecision(6) << clamped_val << std::endl;
        std::cout << debug_prefix << "[DEBUG] Final uint8_t Color value: " << static_cast<int>(result) << std::endl;
    }
    return result;
}

/**
 * @brief 将 8 位无符号颜色通道值转换回浮点数。
 * @param color_uint8 8 位无符号颜色通道值
 * @return 转换后的浮点数
 */
float ps_fp_color_channel_to_float(uint8_t color_uint8) {
    return static_cast<float>(color_uint8);
}


// =========================================================================================
// DDR 数据打包/解包函数 (处理 64 位 ddr_word_t)
// -----------------------------------------------------------------------------------------
// 这些函数根据 HLS 模块中定义的数据布局，将 PS 端数据打包成 64 位 DDR 字，或从 64 位字中解包数据。
// =========================================================================================

// 从 PBIRSM.hpp 获取的 DDR AXI Master 字宽定义
typedef ap_uint<AXI_STREAM_WORD_BITS> ddr_word_t; // 64位

// 从 tbr_rasterizer.hpp 获取的常量
// const int Z_PER_WORD = 4; // 每个 64 位 DDR 字包含 4 个 16 位 Z 值 (ap_uint<16>)
// const int COLOR_PER_WORD = 2; // 每个 64 位 DDR 字包含 2 个颜色像素 (每个 32 位槽)

/**
 * @brief 将 R, G, B 分量打包成 32 位颜色槽 (低 24 位是 BGR)。
 *        用于 PS 端初始化 Frame Buffer 或验证读取。
 *        与 HLS 端打包逻辑保持一致：B (bits 0-7), G (bits 8-15), R (bits 16-23)。
 * @param r, g, b 颜色分量 (0-255)
 * @return 打包后的 32 位颜色槽值 (uint32_t)，高 8 位为 0。
 */
uint32_t ps_pack_rgb_to_32bit_slot(uint8_t r, uint8_t g, uint8_t b) {
    uint32_t packed_color = 0;
    packed_color |= static_cast<uint32_t>(b) << 0;  // B (0-7)
    packed_color |= static_cast<uint32_t>(g) << 8;  // G (8-15)
    packed_color |= static_cast<uint32_t>(r) << 16; // R (16-23)
    return packed_color; // 高 8 位是 0
}

/**
 * @brief 从 32 位颜色槽值中解包 R, G, B 分量。
 *        用于 PS 端从 Frame Buffer 读取后解析颜色。
 *        与 HLS 端打包逻辑保持一致：B (bits 0-7), G (bits 8-15), R (bits 16-23)。
 * @param slot_val 32 位颜色槽值
 * @param r, g, b 输出的 R, G, B 分量
 */
void ps_unpack_32bit_slot_to_rgb(uint32_t slot_val, uint8_t& r, uint8_t& g, uint8_t& b) {
    b = static_cast<uint8_t>((slot_val >> 0) & 0xFF);
    g = static_cast<uint8_t>((slot_val >> 8) & 0xFF);
    r = static_cast<uint8_t>((slot_val >> 16) & 0xFF);
}


// =========================================================================================
// 其他辅助定义和函数
// =========================================================================================

// 定义屏幕分辨率
#define TEST_SCREEN_WIDTH 640
#define TEST_SCREEN_HEIGHT 480
#define TOTAL_PIXELS (TEST_SCREEN_WIDTH * TEST_SCREEN_HEIGHT)

// RGB 结构体，用于帧缓冲区
struct RGB {
    unsigned char b, g, r;
};

// 辅助函数：计算三角形的边缘方程系数 (Ax + By + C = 0)
void calculate_edge_equations(
    float v0x, float v0y, float v1x, float v1y, float v2x, float v2y,
    float& A0_f, float& B0_f, float& C0_f,
    float& A1_f, float& B1_f, float& C1_f,
    float& A2_f, float& B2_f, float& C2_f
) {
    A0_f = v0y - v1y;
    B0_f = v1x - v0x;
    C0_f = v0x * v1y - v1x * v0y;
    
    A1_f = v1y - v2y;
    B1_f = v2x - v1x;
    C1_f = v1x * v2y - v2x * v1y;

    A2_f = v2y - v0y;
    B2_f = v0x - v2x;
    C2_f = v2x * v0y - v0x * v2y;
}

// 辅助函数：计算深度梯度 (dzdx, dzdy)
void calculate_depth_gradients(
    float v0x, float v0y, float v0z, // v0z is 1/Z
    float v1x, float v1y, float v1z,
    float v2x, float v2y, float v2z,
    float& dzdx_f, float& dzdy_f
) {
    float dx1 = v1x - v0x;
    float dy1 = v1y - v0y;
    float dz1 = v1z - v0z; 

    float dx2 = v2x - v0x;
    float dy2 = v2y - v0y;
    float dz2 = v2z - v0z; 

    float det = dx1 * dy2 - dx2 * dy1;

    if (fabs(det) < 1e-6) { 
        dzdx_f = 0.0f;
        dzdy_f = 0.0f;
        return;
    }

    float inv_det = 1.0f / det;
    dzdx_f = (dz1 * dy2 - dz2 * dy1) * inv_det;
    dzdy_f = (dx1 * dz2 - dx2 * dy1) * inv_det;
}

// BMP 文件头结构 (14字节)
#pragma pack(push, 1) 
struct BMPFileHeader {
    unsigned short bfType;      
    unsigned int   bfSize;      
    unsigned short bfReserved1; 
    unsigned short bfReserved2; 
    unsigned int   bfOffBits;   
};

// BMP 信息头结构 (40字节)
struct BMPInfoHeader {
    unsigned int   biSize;          
    int            biWidth;         
    int            biHeight;        
    unsigned short biPlanes;        
    unsigned short biBitCount;      
    unsigned int   biCompression;   
    unsigned int   biSizeImage;     
    int            biXPelsPerMeter; 
    unsigned int   biYPelsPerMeter; 
    unsigned int   biClrUsed;       
    unsigned int   biClrImportant;  
};
#pragma pack(pop)

// 辅助函数：将帧缓冲区数据写入BMP文件
void write_bmp(const char* filename, int width, int height, const std::vector<RGB>& framebuffer) {
    std::ofstream file(filename, std::ios::binary);
    if (!file) {
        std::cerr << "Error: Could not open file " << filename << " for writing." << std::endl;
        return;
    }

    int row_padded_size = (width * 3 + 3) & (~3); 
    int image_size = row_padded_size * height;

    BMPFileHeader file_header;
    file_header.bfType = 0x4D42; 
    file_header.bfSize = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader) + image_size;
    file_header.bfReserved1 = 0;
    file_header.bfReserved2 = 0;
    file_header.bfOffBits = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader);

    BMPInfoHeader info_header;
    info_header.biSize = sizeof(BMPInfoHeader);
    info_header.biWidth = width;
    info_header.biHeight = height; 
    info_header.biPlanes = 1;
    info_header.biBitCount = 24; 
    info_header.biCompression = 0; 
    info_header.biSizeImage = image_size;
    info_header.biXPelsPerMeter = 2835; 
    info_header.biYPelsPerMeter = 2835; 
    info_header.biClrUsed = 0;
    info_header.biClrImportant = 0;

    file.write(reinterpret_cast<char*>(&file_header), sizeof(file_header));
    file.write(reinterpret_cast<char*>(&info_header), sizeof(info_header));
    
    std::vector<unsigned char> row_buffer(row_padded_size);
    for (int y = height - 1; y >= 0; --y) { 
        for (int x = 0; x < width; ++x) {
            row_buffer[x * 3 + 0] = framebuffer[y * width + x].b;
            row_buffer[x * 3 + 1] = framebuffer[y * width + x].g;
            row_buffer[x * 3 + 2] = framebuffer[y * width + x].r;
        }
        file.write(reinterpret_cast<char*>(row_buffer.data()), row_padded_size);
    }

    file.close();
    std::cout << "Successfully wrote " << filename << std::endl;
}


int main() {
    std::cout << "--- GPU_Render Top-Level Module Testbench ---" << std::endl;

    // --- SELF-CHECK: Color Pack/Unpack Logic ---
    std::cout << "\n[TESTBENCH] Self-checking Color Pack/Unpack Logic..." << std::endl;
    uint8_t test_r = 0xAA, test_g = 0xBB, test_b = 0xCC; // Example: R=AA, G=BB, B=CC
    uint32_t packed_slot = ps_pack_rgb_to_32bit_slot(test_r, test_g, test_b);
    uint8_t unpacked_r, unpacked_g, unpacked_b;
    ps_unpack_32bit_slot_to_rgb(packed_slot, unpacked_r, unpacked_g, unpacked_b);

    std::cout << "  Original (R,G,B): (" << std::hex << (int)test_r << "," << (int)test_g << "," << (int)test_b << std::dec << ")" << std::endl;
    std::cout << "  Packed Slot [hex]: " << std::hex << std::setw(8) << std::setfill('0') << packed_slot << std::dec << std::endl;
    std::cout << "  Unpacked (R,G,B): (" << std::hex << (int)unpacked_r << "," << (int)unpacked_g << "," << (int)unpacked_b << std::dec << ")" << std::endl;

    if (test_r == unpacked_r && test_g == unpacked_g && test_b == unpacked_b) {
        std::cout << "  Color pack/unpack self-check PASSED." << std::endl;
    } else {
        std::cerr << "  ERROR: Color pack/unpack self-check FAILED! Mismatch detected." << std::endl;
        // return 1; // If this is a fatal error, you might want to exit early
    }
    std::cout << "-----------------------------------------------------" << std::endl;
    // --- END SELF-CHECK ---

    // --- 1. 模拟 DDR 缓冲区 (使用 std::vector 作为内存块) ---
    // 从 tbr_rasterizer.hpp 和 pbirsm.hpp 获取的常量
    const ap_uint<32> ZBUFFER_ALLOC_WORDS = TEST_SCREEN_WIDTH * TEST_SCREEN_HEIGHT / Z_PER_WORD;
    const ap_uint<32> FRAMEBUFFER_ALLOC_WORDS = TEST_SCREEN_WIDTH * TEST_SCREEN_HEIGHT / COLOR_PER_WORD;
    const ap_uint<32> PARAMS_ALLOC_WORDS = MAX_NUM_TRIANGLES * NUM_AXI_WORDS_PER_PACKET;
    
    std::vector<ddr_word_t> simulated_zbuffer_ddr_storage(ZBUFFER_ALLOC_WORDS);
    std::vector<ddr_word_t> simulated_framebuffer_ddr(FRAMEBUFFER_ALLOC_WORDS);
    std::vector<ddr_word_t> simulated_params_ddr(PARAMS_ALLOC_WORDS);

    // 初始化Z buffer为最远深度 (65535)
    for (ap_uint<32> i = 0; i < ZBUFFER_ALLOC_WORDS; ++i) { 
        simulated_zbuffer_ddr_storage[i] = 0xFFFF'FFFF'FFFF'FFFFULL; // 填充所有 16 位 Z 值为 65535
    }
    std::cout << "[TESTBENCH] Initialized Z-buffer with all 65535 (farthest)." << std::endl;

    // 初始化 Frame buffer 为默认背景色 (灰色 BGR: 0x323232)
    uint32_t default_color_slot = ps_pack_rgb_to_32bit_slot(0x32, 0x32, 0x32); // R=50, G=50, B=50
    for (ap_uint<32> i = 0; i < FRAMEBUFFER_ALLOC_WORDS; ++i) { 
        simulated_framebuffer_ddr[i] = ((ddr_word_t)default_color_slot << 32) | (ddr_word_t)default_color_slot;
    }
    std::cout << "[TESTBENCH] Initialized Frame-buffer with default grey (BGR: 0x323232)." << std::endl;


    // --- 2. 准备三角形数据 (PS端使用浮点数或标准整数) ---
    // 这里我们直接用浮点数定义三角形，并在写入DDR时进行转换。
    struct PSTriangle {
        float v0x, v0y, v0z_norm; // 归一化Z值 [0, 1]
        float v1x, v1y, v1z_norm;
        float v2x, v2y, v2z_norm;
        uint8_t r, g, b; // 纯色
        // Edge equation coefficients, will be calculated later
        float edge0_A, edge0_B, edge0_C;
        float edge1_A, edge1_B, edge1_C;
        float edge2_A, edge2_B, edge2_C;
        // Depth gradients
        float dzdx, dzdy;
    };

    std::vector<PSTriangle> test_triangles_ps;

    // ----- 测试用例 1: 简单的直角三角形 (红色) -----
    test_triangles_ps.push_back({
        50.0f, 50.0f, 0.5f,  // v0 (x,y,z)
        150.0f, 50.0f, 0.6f, // v1 (x,y,z)
        50.0f, 150.0f, 0.7f, // v2 (x,y,z)
        255, 0, 0 // Red (R,G,B)
    });
    
    // ----- 测试用例 2: 扁平的水平三角形 (蓝色), 部分覆盖Case 1，Z值更近 -----
    test_triangles_ps.push_back({
        70.0f, 60.0f, 0.8f,  // v0
        200.0f, 60.0f, 0.85f, // v1
        135.0f, 80.0f, 0.9f,  // v2
        0, 0, 255 // Blue
    });

    // ----- 测试用例 3: 部分在屏幕外的三角形 (绿色), Z值比Case 1更远 -----
    test_triangles_ps.push_back({
        -50.0f, -50.0f, 0.1f, // v0
        50.0f, 0.0f, 0.2f,   // v1
        0.0f, 50.0f, 0.3f,   // v2
        0, 255, 0 // Green
    });

    // ----- 测试用例 4: 一个覆盖大部分屏幕的黄色三角形，Z值在中间 -----
    test_triangles_ps.push_back({
        0.0f, 0.0f, 0.4f,    // v0
        639.0f, 0.0f, 0.45f, // v1
        320.0f, 479.0f, 0.5f, // v2
        255, 255, 0 // Yellow
    });

    // ----- 测试用例 5: 完全在屏幕外的品红色三角形 (不应渲染任何像素) -----
    test_triangles_ps.push_back({
        -200.0f, -200.0f, 0.1f, // v0
        -150.0f, -180.0f, 0.15f, // v1
        -100.0f, -150.0f, 0.2f, // v2
        255, 0, 255 // Magenta
    });
    
    // 计算每个三角形的边缘方程和深度梯度
    for (auto& tri : test_triangles_ps) {
        calculate_edge_equations(tri.v0x, tri.v0y, tri.v1x, tri.v1y, tri.v2x, tri.v2y,
                                 tri.edge0_A, tri.edge0_B, tri.edge0_C,
                                 tri.edge1_A, tri.edge1_B, tri.edge1_C,
                                 tri.edge2_A, tri.edge2_B, tri.edge2_C);
        calculate_depth_gradients(tri.v0x, tri.v0y, tri.v0z_norm,
                                  tri.v1x, tri.v1y, tri.v1z_norm,
                                  tri.v2x, tri.v2y, tri.v2z_norm,
                                  tri.dzdx, tri.dzdy);
    }

    // 确定发送了多少个三角形
    const ap_uint<16> num_triangles_to_process = std::min(static_cast<ap_uint<16>>(test_triangles_ps.size()), static_cast<ap_uint<16>>(MAX_NUM_TRIANGLES)); 
    if (test_triangles_ps.size() > MAX_NUM_TRIANGLES) {
        std::cerr << "WARNING: Original test_triangles size (" << test_triangles_ps.size() << ") exceeds MAX_NUM_TRIANGLES (" << MAX_NUM_TRIANGLES << "). Processing " << num_triangles_to_process << " triangles." << std::endl;
        test_triangles_ps.resize(MAX_NUM_TRIANGLES);
    }


    // --- 3. 准备 PBIRSM 的模拟 DDR 输入 (PS端数据转换为 PL 端位模式并写入模拟DDR) ---
    std::cout << "\n[TESTBENCH] Writing triangle parameters to simulated DDR (PS-to-PL data conversion)..." << std::endl;
    for (int i = 0; i < num_triangles_to_process; ++i) { 
        int current_packet_word_offset = i * NUM_AXI_WORDS_PER_PACKET;
        const PSTriangle& tri = test_triangles_ps[i];
        
        int word_idx = 0; // 当前数据包内的字索引
        bool debug_this_triangle = (i == 0); // 只详细打印第一个三角形

        if (debug_this_triangle) {
            std::cout << "\n-----------------------------------------------------" << std::endl;
            std::cout << "[DEBUG_TRIANGLE " << i << "] PS-Side Original Triangle Data:" << std::endl;
            std::cout << "  v0: (" << std::fixed << std::setprecision(2) << tri.v0x << ", " << tri.v0y << ", Z_norm=" << tri.v0z_norm << ")" << std::endl;
            std::cout << "  v1: (" << tri.v1x << ", " << tri.v1y << ", Z_norm=" << tri.v1z_norm << ")" << std::endl;
            std::cout << "  v2: (" << tri.v2x << ", " << tri.v2y << ", Z_norm=" << tri.v2z_norm << ")" << std::endl;
            std::cout << "  Color: R=" << static_cast<int>(tri.r) << ", G=" << static_cast<int>(tri.g) << ", B=" << static_cast<int>(tri.b) << std::endl;
            std::cout << "  Edge0: A=" << tri.edge0_A << ", B=" << tri.edge0_B << ", C=" << tri.edge0_C << std::endl;
            std::cout << "  Edge1: A=" << tri.edge1_A << ", B=" << tri.edge1_B << ", C=" << tri.edge1_C << std::endl;
            std::cout << "  Edge2: A=" << tri.edge2_A << ", B=" << tri.edge2_B << ", C=" << tri.edge2_C << std::endl;
            std::cout << "  Depth Gradients: dzdx=" << tri.dzdx << ", dzdy=" << tri.dzdy << std::endl;
            std::cout << "-----------------------------------------------------" << std::endl;
            std::cout << "[DEBUG_TRIANGLE " << i << "] PS-to-PL Data Conversion & DDR Write (showing raw bit patterns):" << std::endl;
        }

        // v0_x, v0_y, v1_x, v1_y, v2_x, v2_y (6 * fp_coord_t = 6 words)
        // 写入 fp_coord_t 的内部 int32_t 表示
        int32_t internal_val;
        ddr_word_t ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.v0x, "  v0_x: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val)); // Cast to uint64_t to ensure 64-bit assignment
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  v0_x DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.v0y, "  v0_y: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  v0_y DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }
        
        internal_val = ps_float_to_fp_coord_internal(tri.v1x, "  v1_x: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  v1_x DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.v1y, "  v1_y: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  v1_y DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.v2x, "  v2_x: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  v2_x DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.v2y, "  v2_y: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  v2_y DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }


        // v0_z, v1_z, v2_z (3 * fp_z_buffer_t = 3 words)
        // 将 16 位值放入 64 位字的低 16 位
        uint16_t z_val;

        z_val = ps_float_to_fp_z_buffer((1.0f - tri.v0z_norm) * 65535.0f, "  v0_z: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(z_val); // 高位自动补零
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  v0_z DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        z_val = ps_float_to_fp_z_buffer((1.0f - tri.v1z_norm) * 65535.0f, "  v1_z: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(z_val);
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  v1_z DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        z_val = ps_float_to_fp_z_buffer((1.0f - tri.v2z_norm) * 65535.0f, "  v2_z: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(z_val);
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  v2_z DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }


        // Edge equations (fp_coord_t)
        internal_val = ps_float_to_fp_coord_internal(tri.edge0_A, "  e0_A: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  e0_A DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.edge0_B, "  e0_B: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  e0_B DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.edge0_C, "  e0_C: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  e0_C DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.edge1_A, "  e1_A: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  e1_A DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.edge1_B, "  e1_B: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  e1_B DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.edge1_C, "  e1_C: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  e1_C DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.edge2_A, "  e2_A: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  e2_A DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.edge2_B, "  e2_B: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  e2_B DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.edge2_C, "  e2_C: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  e2_C DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }


        // Depth gradients (fp_coord_t)
        internal_val = ps_float_to_fp_coord_internal(tri.dzdx, "  dzdx: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  dzdx DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        internal_val = ps_float_to_fp_coord_internal(tri.dzdy, "  dzdy: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  dzdy DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }


        // pure_color_R, G, B (fp_color_channel_t) 写入 64 位 DDR 字的低 8 位
        uint8_t color_val;

        color_val = ps_to_fp_color_channel(tri.r, "  Color_R: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(color_val);
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  Color_R DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        color_val = ps_to_fp_color_channel(tri.g, "  Color_G: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(color_val);
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  Color_G DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }

        color_val = ps_to_fp_color_channel(tri.b, "  Color_B: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(color_val);
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        if (debug_this_triangle) { std::cout << "  Color_B DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << ddr_word << std::dec << std::endl; }
        
        if (word_idx != NUM_AXI_WORDS_PER_PACKET) {
            std::cerr << "ERROR: Mismatch in expected words per packet! Expected " << NUM_AXI_WORDS_PER_PACKET << ", but wrote " << word_idx << " for triangle " << i << std::endl;
            return 1; 
        }
        if (debug_this_triangle) {
            std::cout << "-----------------------------------------------------" << std::endl;
        }
    }
    std::cout << "[TESTBENCH] Finished writing " << num_triangles_to_process << " triangles into simulated DDR." << std::endl;
    
    // --- 4. 调用顶层 GPU_Render 模块 (HLS仿真器会自动处理DDR指针的访问) ---
    ap_uint<32> total_write_ops_from_gpu_render = 0;

    std::cout << "\n[TESTBENCH] Calling GPU_Render_TBR top-level module..." << std::endl;
    GPU_Render_TBR(
        simulated_params_ddr.data(),             // params_buffer_base_addr (PS to PL)
        num_triangles_to_process,                // num_triangles_to_process (PS to PL)
        TEST_SCREEN_WIDTH,                       // max_x_res (PS to PL)
        TEST_SCREEN_HEIGHT,                      // max_y_res (PS to PL)
        simulated_zbuffer_ddr_storage.data(),    // zbuffer_ddr_raw_ptr (PL to PS)
        simulated_framebuffer_ddr.data(),        // framebuffer_ddr_raw_ptr (PL to PS)
        total_write_ops_from_gpu_render          // total_write_ops_processed (PL to PS)
    );
    std::cout << "[TESTBENCH] GPU_Render_TBR top-level module finished." << std::endl;

    std::cout << "\n------------------------------------" << std::endl;
    std::cout << "Overall Rasterization Result:" << std::endl;
    std::cout << "GPU_Render_TBR reported total actual DDR writes: " << total_write_ops_from_gpu_render << std::endl;
    std::cout << "------------------------------------" << std::endl;

    // --- 5. 将模拟DDR中的帧缓冲区数据转换为RGB格式并写入BMP文件 (PL到PS数据解析) ---
    std::vector<RGB> final_framebuffer(TOTAL_PIXELS);
    
    // DEBUG: 针对第一个 Tile 的像素进行详细打印
    const int DEBUG_TILE_WIDTH_PX = TILE_WIDTH; // 第一个 Tile 的宽度
    const int DEBUG_TILE_HEIGHT_PX = TILE_HEIGHT; // 第一个 Tile 的高度

    std::cout << "\n[TESTBENCH] PS-Side Reading Framebuffer from simulated DDR & BMP Conversion Debug:" << std::endl;

    for (int i = 0; i < TOTAL_PIXELS; ++i) {
        // 计算当前像素在 64 位 DDR 字中的位置
        ap_uint<32> color_word_idx = i / COLOR_PER_WORD;
        ap_uint<1> color_sub_word_offset = i % COLOR_PER_WORD;

        int current_px_x = i % TEST_SCREEN_WIDTH;
        int current_px_y = i / TEST_SCREEN_WIDTH;

        // 仅对第一个 Tile (0,0) 的像素，并且是每行的前5个和后5个像素进行详细打印
        bool debug_this_pixel = (current_px_y < DEBUG_TILE_HEIGHT_PX && current_px_x < DEBUG_TILE_WIDTH_PX) && 
                                (current_px_x < 5 || current_px_x > DEBUG_TILE_WIDTH_PX - 5 -1); // 考虑到max_x_px是inclusive，所以-1

        if (color_word_idx >= FRAMEBUFFER_ALLOC_WORDS) { 
            final_framebuffer[i] = {0, 0, 0}; // 越界像素设为黑色
            if (debug_this_pixel) {
                std::cout << "[PS_READ_DEBUG] Pixel(" << current_px_x << "," << current_px_y << ") out of bounds for DDR color buffer." << std::endl;
            }
            continue;
        }

        ddr_word_t full_color_word = simulated_framebuffer_ddr[color_word_idx];
        uint32_t packed_pixel_slot; // 32 位颜色槽

        // 根据子字偏移读取对应的 32 位槽
        if (color_sub_word_offset == 0) { // 64位字的低 32 位槽
            packed_pixel_slot = static_cast<uint32_t>(full_color_word & 0xFFFFFFFFULL);
        } else { // 64位字的高 32 位槽
            packed_pixel_slot = static_cast<uint32_t>((full_color_word >> 32) & 0xFFFFFFFFULL);
        }
        
        // 从 32 位槽中解包 24 位 BGR 分量
        uint8_t r, g, b;
        ps_unpack_32bit_slot_to_rgb(packed_pixel_slot, r, g, b);

        final_framebuffer[i] = {b, g, r}; // BMP通常是BGR格式

        // --- DEBUG: 打印 PS 读取的原始 DDR 字和解包结果 ---
        if (debug_this_pixel) { 
            std::cout << "[PS_READ_DEBUG] Pixel(" << current_px_x << "," << current_px_y << "):" << std::endl;
            std::cout << "  DDR Color Word Index: " << color_word_idx << ", Sub-word Offset: " << (int)color_sub_word_offset << std::endl;
            std::cout << "  Raw DDR Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << full_color_word << std::dec << std::endl;
            std::cout << "  Packed Pixel Slot [hex]: " << std::hex << std::setw(8) << std::setfill('0') << packed_pixel_slot << std::dec << std::endl;
            std::cout << "  Unpacked Color (R,G,B): (" << (int)r << "," << (int)g << "," << (int)b << ")" << std::endl;

            // 同时检查 Z-buffer
            ap_uint<32> z_word_idx = i / Z_PER_WORD;
            ap_uint<3> z_sub_word_offset = i % Z_PER_WORD;
            if (z_word_idx < ZBUFFER_ALLOC_WORDS) {
                ddr_word_t full_z_word = simulated_zbuffer_ddr_storage[z_word_idx];
                uint16_t unpacked_z = static_cast<uint16_t>((full_z_word >> (z_sub_word_offset * 16)) & 0xFFFFULL);
                std::cout << "  Raw DDR Z Word [hex]: " << std::hex << std::setw(16) << std::setfill('0') << full_z_word << std::dec << std::endl;
                std::cout << "  Unpacked Z: " << unpacked_z << std::endl;
            } else {
                std::cout << "  Z-Buffer: Pixel outside Z buffer bounds." << std::endl;
            }
            std::cout << "--------------------------------------------------------" << std::endl;
        }
        // --- END DEBUG ---
    }

    // Fix a typo in write_bmp: sizeof(&info_header) should be sizeof(info_header)
    // The current fix is directly applied in the write_bmp function above.

    write_bmp("raster_output_combined.bmp", TEST_SCREEN_WIDTH, TEST_SCREEN_HEIGHT, final_framebuffer);

    return 0; // 成功返回 0
}
